import os
import re

file_names = []
ros2_file_names = []

input_path = './ros_file'  # 替换为你的文件夹路径
out_path = './ros2_file'

pubs = []
pubs_dic = {}

subs = []
subs_dic_sub_back = {}
subs_dic_back_type = {}

msg_names = []
msg_names_underscore = []
global_callback_func = []
service_names = []
service_names_var = {}
callback_strs = ['Wrap', 'allback', 'allBack']
param_strs = ['_nh.param','n.param','nh_.param','_nh.getParam','private_nh_.getParam','nh_private_.param','nh_private_.getParam']
callback_funcStr_data = {}
params_var_type={}


def get_files_in_folder(path):
    if not is_sub_file(path):  # 如果没有子目录，就把子文件全部加入，结束递归
        for file in os.listdir(path):
            file = path + '/' + file
            file_names.append(file)
        return

    for file in os.listdir(path):
        file = path + '/' + file
        if not os.path.isdir(file):
            file_names.append(file)
        else:
            for item in os.listdir(file):
                file_path = file + '/' + item
                # print(file_path)

                if os.path.isdir(file_path):  # 判断没有子目录，就加上退出
                    # print("是目录")
                    # print(file_path)
                    get_files_in_folder(file_path)
                else:
                    # print("不是目录，是文件")
                    file_names.append(file_path)

    for i in file_names:
        new_i = i.replace(input_path,out_path)
        ros2_file_names.append(new_i)


def is_float_string(s):
    try:
        float(s)
        return True
    except ValueError:
        return False

def is_sub_file(path):
    if os.path.isdir(path):
        has_subdirs = False
        for filename in os.listdir(path):
            subdir_path = os.path.join(path, filename)
            if os.path.isdir(subdir_path):
                has_subdirs = True
                break
        if has_subdirs:
            return True
        else:
            return False

# 首字母小写
def lower_first_letter(s):
    return s[:1].lower() + s[1:]


def is_all_spaces(string):
    for char in string:
        if not char.isspace():
            return False
    return True

def camel_to_underscore(name):
    tag = 0
    result = ''
    for i in range(len(name)):
        if name[i] == '\n':
            tag = 1
        if name[i] == '/':
            tag = 2
        if name[i] == '#':
            tag = 3
        if name[i] == ' ' and tag == 2:
            tag = 4

        if name[i].isupper() and tag not in [2,3]:
            result += '_' + name[i].lower()
        else:
            result += name[i]
    return result.lstrip('_')

def common_replace(m_str):
    # 基础替换
    m_str = m_str.replace('ubt_com/ubt_com_util.h', 'ubt_utils/ubt_com_util.h')
    m_str = m_str.replace('ubt_com/cJSON.h', 'ubt_utils/cJSON.h')
    m_str = m_str.replace('ubt_com/ubt_proto_adjust.h', 'ubt_utils/ubt_proto_adjust.h')
    m_str = m_str.replace('ubt_com/ubt_proto_control.h', 'ubt_utils/ubt_proto_control.h')
    m_str = m_str.replace('ubt_com/ubt_proto_nav.h', 'ubt_utils/ubt_proto_nav.h')
    m_str = m_str.replace('ubt_com/ubt_com_defines.h', 'ubt_utils/ubt_com_defines.h')
    m_str = m_str.replace('ubt_com/ubt_com_msg_head_api.h', 'ubt_utils/ubt_com_msg_head_api.h')
    m_str = m_str.replace('json/json.h', 'jsoncpp/json/json.h')

    # ros sub pub client service
    m_str = m_str.replace('ros::ServiceClient', 'rclcpp::Client<>')
    m_str = m_str.replace('pNh.serviceClient', 'node->create_client')
    m_str = m_str.replace('pNh->serviceClient', 'node->create_client')
    m_str = m_str.replace('nh.serviceClient', 'node->create_client')
    m_str = m_str.replace('_nh.serviceClient', 'node->create_client')
    m_str = m_str.replace('_pNodeHandle->serviceClient', 'node->create_client')

    m_str = m_str.replace('ros::ServiceServer', 'rclcpp::Service')
    m_str = m_str.replace('pNh.advertiseService', 'node->create_service')
    m_str = m_str.replace('pNh->advertiseService', 'node->create_service')
    m_str = m_str.replace('nh.advertiseService', 'node->create_service')
    m_str = m_str.replace('_nh.advertiseService', 'node->create_service')
    m_str = m_str.replace('nh_private_.advertiseService', 'node->create_service')


    # 删除替换
    # m_str = m_str.replace('#include "unav/rate.h"', '')
    m_str = m_str.replace('const ros::TimerEvent& event', '')
    m_str = m_str.replace('#include "ros/ros.h"', '')
    m_str = m_str.replace('ros::NodeHandle pNh;', '')
    m_str = m_str.replace('ros::NodeHandle *pNh,', '')
    m_str = m_str.replace('ros::NodeHandle *pNh', '')
    m_str = m_str.replace('ros::NodeHandle &nh', '')
    m_str = m_str.replace(': nh_(nh)', '')
    m_str = m_str.replace('ros::NodeHandle n;', '')
    m_str = m_str.replace('ros::NodeHandle n', '')
    m_str = m_str.replace('#include <ros/package.h>', '')

    m_str = m_str.replace('::ConstPtr', '')
    m_str = m_str.replace('# include <ros/package.h>', '')
    m_str = m_str.replace('# include <rospack/rospack.h>', '')

    m_str = m_str.replace('ros::NodeHandle', '// ros::NodeHandle')



    # ros相关替换
    m_str = m_str.replace('&_nh', 'node')
    m_str = m_str.replace('ros::init', 'rclcpp::init')
    m_str = m_str.replace('ros::spinOnce', 'rclcpp::spin_some')
    m_str = m_str.replace('ros::ok', 'rclcpp::ok')
    m_str = m_str.replace('ros::Timer', 'rclcpp::TimerBase::SharedPtr')
    m_str = m_str.replace('ros::WallTimer', 'rclcpp::TimerBase::SharedPtr')
    m_str = m_str.replace('ros::Time', 'rclcpp::Time')
    m_str = m_str.replace('ros::Rate', 'rclcpp::Rate')
    m_str = m_str.replace('.sec', '.seconds()')
    m_str = m_str.replace('.nsec', '.nanoseconds()')
    m_str = m_str.replace('#include <ros/ros.h>', '')
    # 处理tf
    m_str = m_str.replace('tf/', 'tf2_ros/')





    m_str = m_str.replace('tf2_ros/transform_datatypes', 'tf2/transform_datatypes')



    m_str = m_str.replace('ros::Publisher', 'rclcpp::Publisher')
    m_str = m_str.replace('ros::Subscriber', 'rclcpp::Subscription')
    m_str = m_str.replace('ros::spin()', 'rclcpp::spin(node)')
    m_str = m_str.replace('nh.advertise', 'node->create_publisher')
    m_str = m_str.replace('nh.subscribe', 'node->create_subscription')
    m_str = m_str.replace('nh_.advertise', 'node->create_publisher')
    m_str = m_str.replace('nh_.subscribe', 'node->create_subscription')
    m_str = m_str.replace('_pNodeHandle->advertise', 'node->create_publisher')
    m_str = m_str.replace('_pNodeHandle->subscribe', 'node->create_subscription')
    m_str = m_str.replace('pNh->advertise', 'node->create_publisher')
    m_str = m_str.replace('pNh->subscribe', 'node->create_subscription')
    m_str = m_str.replace('n.advertise', 'node->create_publisher')
    m_str = m_str.replace('n.subscribe', 'node->create_subscription')
    m_str = m_str.replace('.publish(', '->publish(')
    m_str = m_str.replace('ros::package::getPath', 'ament_index_cpp::get_package_share_directory')

    # m_str = m_str.replace('nh.subscribe', 'node->create_subscription')
    # ros-log
    m_str = m_str.replace('ROS_INFO(', 'RCLCPP_INFO(node->get_logger(),')
    m_str = m_str.replace('ROS_WARN(', 'RCLCPP_WARN(node->get_logger(),')
    m_str = m_str.replace('ROS_ERROR(', 'RCLCPP_ERROR(node->get_logger(),')
    m_str = m_str.replace('ROS_DEBUG(', 'RCLCPP_DEBUG(node->get_logger(),')
    m_str = m_str.replace('ROS_INFO_STREAM(', 'RCLCPP_INFO_STREAM(node->get_logger(),')
    m_str = m_str.replace('ROS_WARN_STREAM(', 'RCLCPP_WARN_STREAM(node->get_logger(),')
    m_str = m_str.replace('ROS_ERROR_STREAM(', 'RCLCPP_ERROR_STREAM(node->get_logger(),')
    m_str = m_str.replace('ROS_DEBUG_STREAM(', 'RCLCPP_DEBUG_STREAM(node->get_logger(),')

    # 处理消息类型


    m_str = m_str.replace('std_srvs::', 'std_srvs::srv::')
    m_str = m_str.replace('geometry_msgs::', 'geometry_msgs::msg::')
    m_str = m_str.replace('std_msgs::', 'std_msgs::msg::')
    m_str = m_str.replace('visualization_msgs::', 'visualization_msgs::msg::')
    m_str = m_str.replace('sensor_msgs::', 'sensor_msgs::msg::')
    m_str = m_str.replace('nav_msgs::', 'nav_msgs::msg::')


    # 转换后再替换
    m_str = m_str.replace('rclcpp::Time::now()', 'rclcpp::Clock().now()')
    m_str = m_str.replace('.toSec()', '.seconds()')

    # 单行的处理

    replace_hpp_name = ['std_msgs/msg/', 'geometry_msgs/msg/', 'nav_msgs/msg/', 'sensor_msgs/msg/',
                        'std_srvs/srv/','visualization_msgs/msg/']
    new_str = ''
    global pubs
    global subs
    # 获取pubs,subs等
    for line_str_cpp in m_str.split('\n'):
        if ('rclcpp::Publisher' in line_str_cpp):
            pubs.append(line_str_cpp.split('Publisher ')[1].split(';')[0])
        if ('rclcpp::Subscription' in line_str_cpp):
            if len(line_str_cpp.split('Subscription '))>=2:
                sub = line_str_cpp.split('Subscription ')[1].split(';')[0]
                if 'create_subscription' in sub:
                    sub = sub.split('=')[0].strip()
                subs.append(sub)
            else:
                print(line_str_cpp)

    pubs = list(set(pubs))# 去重z
    subs = list(set(subs))# 去重

    # 获取pubs_dic,subs_dic_sub_back

    global subs_dic_sub_back
    global subs_dic_back_type
    for line_str_cpp in m_str.split('\n'):
        for pub in pubs:
            if pub in line_str_cpp and 'create_publisher' in line_str_cpp:
                # print(line_str_cpp.split('<')[1].split('>')[0])
                global pubs_dic
                if pub not in pubs_dic:
                    pubs_dic[pub] = line_str_cpp.split('<')[1].split('>')[0]
        for sub in subs:
            # if sub in line_str_cpp:
            #     if 'create_subscription' in line_str_cpp:

            if sub in line_str_cpp and 'create_subscription' in line_str_cpp:
                if len(line_str_cpp.split('&'))>=2:
                    call_back_func_name = line_str_cpp.split('&')[1]
                else:
                    call_back_func_name = line_str_cpp.split(',')[2].split(');')[0]
                    # print(call_back_func_name)
                if '::' in call_back_func_name:
                    call_back_func_name = call_back_func_name.split('::')[1].split(',')[0]
                else:
                    call_back_func_name = call_back_func_name.split(');')[0]
                # print(call_back_func_name)
                if sub not in subs_dic_sub_back:
                    subs_dic_sub_back[sub] = call_back_func_name

    # 获取pubs_dic,subs_dic_back_type
    for line_str_cpp in m_str.split('\n'):
        for callback_name in subs_dic_sub_back.values():
            if callback_name in line_str_cpp and 'void' in line_str_cpp:
                if len(line_str_cpp.split('const '))>=2:
                    sub_type = line_str_cpp.split('const ')[1].split('&')[0]
                else:
                    pass
                    # print(line_str_cpp)
                # print(line_str_cpp.split('const ')[1].split('&')[0])
                if callback_name not in subs_dic_back_type:
                    if callback_name in subs_dic_back_type.keys():
                        subs_dic_back_type[callback_name] = sub_type
                    else:
                        sub_type = line_str_cpp.split('(')[1].split(')')[0].split(' ')[0]
                        subs_dic_back_type[callback_name] = sub_type
                        # print(sub_type)

                # else:
                    # print('not in ---'+callback_name)

    for line_str_cpp in m_str.split('\n'):
        #处理msg头文件换成hpp
        for hpp_name_value in replace_hpp_name:
            if hpp_name_value in line_str_cpp:
                line_str_cpp = line_str_cpp.replace('.h>', '.hpp>')
                msg_type = line_str_cpp.split('.hpp')[0].split('/')[-1]
                line_str_cpp = line_str_cpp.replace(msg_type, camel_to_underscore(msg_type))
                # print(camel_to_underscore(msg_type))
        if ('NodeHandle' in line_str_cpp) or ('NodeHandle' in line_str_cpp):
            line_str_cpp = line_str_cpp.replace('ros::NodeHandle *pNh','');

        # 处理pub变量
        # if ('rclcpp::Publisher' in line_str_cpp) :

            # print(line_str_cpp.split('Publisher ')[1].split(';')[0])

        # 处理回调函数-收集回调函数名
        for callback_str in callback_strs:
            if callback_str in line_str_cpp:
                # 处理sub回调定义
                if '&' in line_str_cpp and 'void' in line_str_cpp:
                    if len(line_str_cpp.split('void '))>=2:
                        callback_name = line_str_cpp.split('void ')[1].split('(')[0].strip()
                        # print(callback_name)
                        global_callback_func.append(callback_name)
                    else:
                        # print(line_str_cpp)
                        pass
                elif ';' in line_str_cpp and 'void' in line_str_cpp:
                    # print(line_str_cpp)
                    if len(line_str_cpp.split('::')) >= 2:
                        callback_name = line_str_cpp.split('void')[1].split('(')[0].strip()
                        # print(callback_name)
                        global_callback_func.append(callback_name)
                    else:
                        # print(line_str_cpp)
                        pass




    for line_str_cpp in m_str.split('\n'):
        if '.shutdown(' in line_str_cpp:
            line_str_cpp = '// '+line_str_cpp
        # 处理msg头文件换成hpp

        if 'ubt_com/' in line_str_cpp:
            msg_name = line_str_cpp.split('.h')[0].split('/')[-1]
            new_msg_name = camel_to_underscore(lower_first_letter(msg_name))
            # print(new_msg_name)
            line_str_cpp = line_str_cpp.split(msg_name)[0]+new_msg_name+'.hpp"'
            # print(line_str_cpp)
            line_str_cpp = line_str_cpp.replace('ubt_com/', 'ubt_interface/msg/')

        msg_names = ['std_msgs','geometry_msgs','nav_msgs','sensor_msgs','std_srvs','visualization_msgs']

        for name in msg_names:
            if 'ubt_com::' in line_str_cpp:
                # print(line_str_cpp)
                if 'Request' in line_str_cpp or 'Response' in line_str_cpp:
                    line_str_cpp = line_str_cpp.replace('ubt_com::', 'ubt_interface::srv::')
                else:
                    line_str_cpp = line_str_cpp.replace('ubt_com::', 'ubt_interface::msg::')
            new_name = name +'/'
            if new_name in line_str_cpp:
                msg_or_srv = ''
                type_name =line_str_cpp.split('/')[1].split('.h')[0]
                if 'msg' in new_name:
                    msg_or_srv ='msg/'
                if 'srv' in new_name:
                    msg_or_srv ='srv/'
                line_str_cpp = line_str_cpp.split(name)[0] + new_name+msg_or_srv+ line_str_cpp.split(new_name)[1]
                line_str_cpp = line_str_cpp.replace('.h', '.hpp');
                line_str_cpp = line_str_cpp.replace(type_name, camel_to_underscore(type_name));
                # print(line_str_cpp)


        new_str = new_str + line_str_cpp + '\n'

    return new_str
def ros_to_uauto_msgOrSrv():
    h_files = []
    for file in file_names:
        # print(file)
        if os.path.splitext(file)[1] == '.msg' or os.path.splitext(file)[1] == '.srv':
            h_files.append(file)

    for file in h_files:
        with open(file, 'r', encoding='utf-8') as f:
            cpp_str = f.read()
            # 文件内容操作

        file = file.replace(input_path, out_path)
        file_dir = file.replace(file.split('/')[-1], '')
        if not os.path.exists(file_dir):
            os.makedirs(file_dir)
        with open(file, 'w', encoding='utf-8') as f:
            f.write(cpp_str)
def ros_to_uauto_launchOrCfg():
    h_files = []
    for file in file_names:
        # print(file)
        if os.path.splitext(file)[1] == '.launch' or os.path.splitext(file)[1] == '.yaml' or os.path.splitext(file)[1] == '.txt':
            h_files.append(file)


    for file in h_files:
        with open(file, 'r', encoding='utf-8') as f:
            cpp_str = f.read()
            # 文件内容操作

        file = file.replace(input_path, out_path)
        file_dir = file.replace(file.split('/')[-1], '')
        if not os.path.exists(file_dir):
            os.makedirs(file_dir)
        with open(file, 'w', encoding='utf-8') as f:
            f.write(cpp_str)

def ros_to_uauto_h():
    h_files = []
    for file in file_names:
        # print(file)
        if os.path.splitext(file)[1] == '.h':
            h_files.append(file)


    for file in h_files:
        with open(file, 'r', encoding='utf-8') as f:
            cpp_str = f.read()
            # 文件内容操作
            # 共同替换
            cpp_str = common_replace(cpp_str)



        file = file.replace(input_path, out_path)
        file_dir = file.replace(file.split('/')[-1], '')
        if not os.path.exists(file_dir):
            os.makedirs(file_dir)
        with open(file, 'w', encoding='utf-8') as f:
            f.write(cpp_str)


def ros_to_uauto_cpp():
    cpp_files = []
    for file in file_names:
        if os.path.splitext(file)[1] == '.cpp':
            cpp_files.append(file)
            # print(file)

    for file in cpp_files:
        timers_replaces = {}
        with open(file, 'r', encoding='utf-8') as f:
            cpp_str = f.read()
            # 共同替换
            cpp_str = common_replace(cpp_str)
            # 拆分重组替换
            new_str = ''
            for line_str_cpp in cpp_str.split('\n'):
                if 'rclcpp::init(argc' in line_str_cpp:
                    space = line_str_cpp.split('rclcpp::init(argc')[0]
                    node_name = line_str_cpp.split('"')[1].split('"')[0]
                    line_str_cpp = space+line_str_cpp.split('"'+node_name)[0][:-1]+line_str_cpp.split(node_name+'"')[1]
                    line_str_cpp = line_str_cpp +'\n'+space+'node =  std::make_shared<rclcpp::Node>("'+node_name+'");'
                    # print(line_str_cpp)

                # 处理参数
                if 'ros::param::param' in line_str_cpp and ';' in line_str_cpp:
                    space =line_str_cpp.split('ros::param::param')[0]
                    var_name = line_str_cpp.split('",')[1].split(',')[0].strip()
                    param_name = line_str_cpp.split('",')[0].split('("')[1]

                    param_type = line_str_cpp.split('<')[1].split('>')[0];

                    if param_type == 'float':
                        param_type = 'double'
                    if len(line_str_cpp.split(');')[0].split(var_name)) >=2:
                        # print(line_str_cpp.split(');')[0].split(var_name+',')[-1].strip())
                        param_default_value =line_str_cpp.split(');')[0].split(var_name+',')[-1].strip()
                    else:
                        # print(line_str_cpp)
                        pass
                    # print(param_default_value)
                    if (param_type == 'string'):
                        param_type = 'std::string'
                    declare = 'node->declare_parameter<'+ param_type +'>("'+param_name+'",'+param_default_value+');\n'
                    if (param_type == 'std::string'):
                        param_type = 'string'
                    line_str_cpp = space + declare +\
                        space + var_name + ' = node->get_parameter("' + param_name + '").as_'+param_type+'();'
                    # print(param_name)
                for param_str in param_strs:

                    if param_str in line_str_cpp:
                        if ';' in line_str_cpp:
                            space = line_str_cpp.split(param_str)[0]
                            if not is_all_spaces(space):
                                str_not_space = space.strip()
                                space = space.split(str_not_space)[0]
                                # print(space)
                            param_type = 'no_type'
                            if '=' in line_str_cpp:
                                var_name = line_str_cpp.split('=')[0].strip()
                                # print(var_name)
                            else:
                                var_name = line_str_cpp.split(',')[1].split(',')[0].strip()
                                if ');' in var_name:
                                    var_name = var_name.split(');')[0]




                            param_name = line_str_cpp.split('",')[0].split('"')[1]
                            # print(param_name)
                            if '<' in line_str_cpp:
                                param_type = line_str_cpp.split('<')[1].split('>')[0];


                            if param_type == 'float':
                                param_type = 'double'
                            param_default_value = line_str_cpp.split(',')[-1].split(');')[0]



                            if param_type == 'no_type':
                                param_var = var_name
                                if '.' in var_name:
                                    param_var = var_name.split('.')[1]
                                if param_var in params_var_type.keys():

                                    param_type = params_var_type[param_var]
                                    # print(var_name+"--get---"+param_type)

                            if (param_type == 'string'):
                                param_type = 'std::string'
                            declare = 'node->declare_parameter<'+ param_type +'>("' + param_name + '",' + param_default_value + ');\n'
                            if (param_type == 'std::string'):
                                param_type = 'string'


                            # print(param_type)
                            line_str_cpp = space + declare + \
                                           space + var_name + ' = node->get_parameter("' + param_name + '").as_' + param_type + '();'
                            # print(var_name)
                        elif 'getParam' in line_str_cpp:

                            line_str_cpp = line_str_cpp.replace(param_str,'node->get_parameter')
                            # print(line_str_cpp)

                # 处理duration
                if 'ros::Duration' in line_str_cpp and 'sleep' in line_str_cpp:
                    num = line_str_cpp.split('Duration(')[1].split(')')[0]
                    if not num.isdigit():
                        # print(line_str_cpp)
                        space = line_str_cpp.split('ros::Duration')[0]
                        line_str_cpp = space+'rclcpp::sleep_for(std::chrono::milliseconds('+ num*1000 +'));'
                    else:
                        seconds = float(num)
                        space = line_str_cpp.split('ros::Duration')[0]
                        line_str_cpp = space+'rclcpp::sleep_for(std::chrono::milliseconds('+ str(int(seconds*1000)) +'));'
                    # print(line_str_cpp)


                for param_str in param_strs:
                    if param_str in line_str_cpp and ';' in line_str_cpp:
                        space = line_str_cpp.split(param_str)[0]
                        var_name = line_str_cpp.split('",')[1].split(',')[0]
                        param_name = line_str_cpp.split('",')[0].split('>("')[1]
                        param_type = line_str_cpp.split('<')[1].split('>')[0]
                        if (param_type == 'std::string'):
                            param_type = 'string'
                        param_default_value = line_str_cpp.split(');')[0].split(var_name)[-1].split(',')[1]
                        declare = 'node->declare_parameter("' + param_name + '",' + param_default_value + ');\n'
                        line_str_cpp = space + declare + \
                                       space + var_name + ' = node->get_parameter("' + param_name + '").as_' + param_type + '();'

                    # print(param_type)

                for callback_str in callback_strs:
                    if callback_str in line_str_cpp   and ';' in line_str_cpp:
                        # 处理sub回调定义
                        if '&' in line_str_cpp:
                            if 'Wrap' in line_str_cpp:
                                callback_name = line_str_cpp.split('&')[1].split(');')[0]
                                line_str_cpp = line_str_cpp.split('&')[0] + 'std::bind(&' + callback_name + ', std::placeholders::_1));'
                                # print(line_str_cpp)
                            else:
                                # print(line_str_cpp)
                                callback_name = line_str_cpp.split('&')[1].split(',')[0]
                                line_str_cpp = line_str_cpp.split('&')[0] + 'std::bind(&' + callback_name + ', this, std::placeholders::_1));'
                        else:
                            if 'create_subscription' in line_str_cpp:
                                callback_name = line_str_cpp.split(',')[2].split(')')[0].strip()
                                line_str_cpp = line_str_cpp.split(callback_name)[0]+ 'std::bind(' + callback_name + ', std::placeholders::_1)'+line_str_cpp.split(callback_name)[1]
                                # print(line_str_cpp)


                new_str = new_str + line_str_cpp + '\n'

            # 处理定时器
            for line_str_cpp in new_str.split('\n'):
                if 'createTimer' in line_str_cpp:
                    # print(line_str_cpp)
                    head = line_str_cpp.split('=')[0]
                    time = line_str_cpp.split('ros::Duration(')[1].split(')')[0]
                    if is_sub_file(time):
                        time = int(float(line_str_cpp.split('ros::Duration(')[1].split(')')[0]) *1000)
                        # print(line_str_cpp)
                    else:
                        # pass
                        if '(' not in time:
                            time = 'int(('+time+')*1000)'
                        # print(time)
                    if len(line_str_cpp.split('&'))>=2:
                        func = line_str_cpp.split('&')[1].split(',')[0]
                    else:
                        if 'createTimer' in line_str_cpp:
                            # print(line_str_cpp)
                            continue
                    if '::' in line_str_cpp:
                        replace_str = head +' = node->create_wall_timer(std::chrono::milliseconds('+str(time)+'),std::bind(&'+func+', this));'
                    else:
                        replace_str = head + ' = node->create_wall_timer(std::chrono::milliseconds(' + str(
                            time) + '),std::bind(&' + func + '));'
                        pass
                    # print(replace_str)
                    new_str = new_str.replace(line_str_cpp, replace_str)
            # 处理回调函数

            for line_str_cpp in new_str.split('\n'):
                for callback_name in global_callback_func:
                    if callback_name in line_str_cpp and 'void' in line_str_cpp:
                        # print(line_str_cpp)
                        for callback_str in callback_strs:
                            callback_str = callback_str+'('
                            if callback_str in line_str_cpp:
                                # print(line_str_cpp)

                                callback_type = line_str_cpp.split(callback_str)[1].split('&')[0].strip() + '::SharedPtr '

                                if len(line_str_cpp.split('&'))>=2:
                                    call_back_name = line_str_cpp.split(callback_str)[0] + callback_str + callback_type + \
                                                     line_str_cpp.split('&')[1].strip()
                                else:
                                    # print(line_str_cpp)
                                    if 'static' in line_str_cpp:
                                        continue
                                    else:
                                        msg_datas = line_str_cpp.split(callback_str)[1].split(')')[0].strip()
                                        callback_type =  'const '+msg_datas.split(' ')[0] + '::SharedPtr '+msg_datas.split(' ')[1]
                                        call_back_name = line_str_cpp.split(callback_str)[0] + callback_str + callback_type + ')'+ \
                                                     line_str_cpp.split(')')[1].strip()



                                # print(line_str_cpp+ '                          '+call_back_name)
                                new_str = new_str.replace(line_str_cpp, call_back_name)

            # 处理service
            # 获取service_name
            global service_names
            global service_names_var
            for line_str_cpp in new_str.split('\n'):
                if 'create_client' in line_str_cpp:
                    service_name = line_str_cpp.split('"')[1]
                    if 'create_client' in line_str_cpp :
                        var_name = line_str_cpp.split('=')[0].strip()
                        if '//' in var_name:
                            var_name = var_name.split('//')[1].strip()
                        service_names_var[service_name] = var_name

                        # print(var_name)
                    service_names.append(service_name)
            # print(service_names_var)
            for line_str_cpp in new_str.split('\n'):
                for service_name in service_names:
                    # print(line_str_cpp)
                    if service_name in line_str_cpp:
                        if 'ros::service::waitForService' in line_str_cpp:
                            space = line_str_cpp.split('ros::service::waitForService')[0]
                            replace_str = space+ 'while (!'+service_names_var[service_name]+'->wait_for_service(1s)) { \n' \
                                        +space+'  RCLCPP_INFO(node->get_logger(),"'+service_name +' service not available, waiting again...");\n' \
                                        +space+'}';

                            # print(replace_str)
                            new_str = new_str.replace(line_str_cpp,replace_str)
                            # pass
                for service_var in service_names_var.values():
                    if service_var in line_str_cpp:
                        if '.call' not in line_str_cpp:
                            # print(line_str_cpp)
                            pass
                        if '.call' in line_str_cpp:
                            replace_str = line_str_cpp.replace('.call(req, rsp)','->async_send_request(req)')
                            # print(line_str_cpp)
                            new_str = new_str.replace(line_str_cpp, replace_str)



            # 处理callback函数中.边成->
            callback_data_name = ''
            global callback_funcStr_data
            content_contain_count = 0
            start_trains = False
            for line_str_cpp in new_str.split('\n'):
                for callback_str in callback_strs:
                    if callback_str in line_str_cpp  and 'void' in line_str_cpp:
                        # print(line_str_cpp)
                        split_str = line_str_cpp.split('SharedPtr ')
                        if len(split_str)>=2:
                            callback_data_name = split_str[1].split(')')[0]
                            callback_funcStr_data[line_str_cpp] = callback_data_name
                            # print(line_str_cpp.split('void')[1].split('(')[0].strip())

            for line_str_cpp in new_str.split('\n'):
                # print(line_str_cpp)
                replace_str = line_str_cpp
                for func_str in callback_funcStr_data.keys():
                    if line_str_cpp == func_str:
                        start_trains = True
                        callback_data_name = line_str_cpp.split('::SharedPtr')[1].split(')')[0].strip()

                if '{' in line_str_cpp and start_trains:
                    content_contain_count +=1
                if '}' in line_str_cpp and start_trains: # 结束的时候替换
                    content_contain_count -=1
                    if content_contain_count == 0:
                        start_trains = False
                # print(line_str_cpp)

                if start_trains:
                    # print(line_str_cpp+'                        '+callback_data_name)

                    pattern = re.compile(r'.*?_msg..*')

                    if pattern.match(line_str_cpp):

                        # print(line_str_cpp)
                        pass
                    else:
                        # if 'controlParamInfo_.lift_flag=msg' in line_str_cpp:
                        #     print(line_str_cpp + "------------------111-")
                        #     print(callback_data_name)

                        replace_str = line_str_cpp.replace(callback_data_name + '.', callback_data_name + '->')
                    # print(replace_str)

                new_str = new_str.replace(line_str_cpp, replace_str)





            cpp_str = new_str


        file = file.replace(input_path, out_path)
        file_dir = file.replace(file.split('/')[-1], '')
        if not os.path.exists(file_dir):
            os.makedirs(file_dir)
        with open(file, 'w', encoding='utf-8') as f:
            f.write(cpp_str)


def ros_to_uauto_other():
    # 调试打印pubs
    # print('*************************************************************** pubs_dict **************************************************************')
    # print(len(pubs))
    # print(len(pubs_dic))
    # for k in pubs:
    #     if k not in pubs_dic.keys():
    #         print(k +" ----------------------------not in pubs_dic")
    #     else:
    #         print("k: "+k+"                     v: "+pubs_dic[k])
    # 调试打印subs

    # print('*************************************************************** subs_dict **************************************************************')
    # print(len(subs))
    # print(len(subs_dic_sub_back))
    # print(len(subs_dic_back_type))
    # for k in subs:
    #     if k in subs_dic_sub_back.keys():
    #         if subs_dic_sub_back[k] in subs_dic_back_type.keys():
    #             print("k: "+k+"                     func: "+subs_dic_sub_back[k]+"                     type: "+subs_dic_back_type[subs_dic_sub_back[k]])
    #         else:
    #             print("------------------------error--subs_dic_sub_back--k: "+k + '          func: '+subs_dic_sub_back[k])
    #     else:
    #         print('---------------------error--subs--'+k)

    h_files = []
    for file in ros2_file_names:
        # print(file)
        if os.path.splitext(file)[1] == '.h' or os.path.splitext(file)[1] == '.cpp':
            h_files.append(file)

    for file in h_files:
        # print(file)
        with open(file, 'r', encoding='utf-8') as f:
            cpp_str = f.read()
            cpp_str = cpp_str.replace(', true);', ');')
            cpp_str = cpp_str.replace('seconds()ond', 'second')
            cpp_str = cpp_str.replace('const rclcpp::TimerBase::SharedPtrEvent &te', '')
            cpp_str = cpp_str.replace('create_publisherService', 'create_service')
            # cpp_str = cpp_str.replace('rclcpp::TimerBase::SharedPtr', '')



            for line_str_cpp in cpp_str.split('\n'):
                if 'rclcpp::TimerBase::SharedPtrEvent' in line_str_cpp:
                    replace_str = line_str_cpp.replace('const rclcpp::TimerBase::SharedPtrEvent& e', '')
                    cpp_str = cpp_str.replace(line_str_cpp, replace_str)
                # 处理publisher
                if 'create_publisher' in line_str_cpp and ',true' in line_str_cpp:
                    replace = line_str_cpp.split(',true')[0]+line_str_cpp.split(',true')[1]
                    cpp_str = cpp_str.replace(line_str_cpp, replace)
                    # print(replace)

                if 'TimerEvent' in line_str_cpp:
                    # print(line_str_cpp)
                    cpp_str = cpp_str.replace('const rclcpp::TimerEvent& te','')
                    cpp_str = cpp_str.replace('_node->','node->')
                # if 'ubt'
                # print(global_callback_func)

                for callback_name in global_callback_func:
                    # print(callback_name)
                    if callback_name in line_str_cpp and 'void' in line_str_cpp:
                        if '::SharedPtr' not in line_str_cpp:
                            for callback_str in callback_strs:
                                callback_str = callback_str+'('
                                if callback_str in line_str_cpp :
                                    # print(line_str_cpp)
                                    if '::SharedPtr' in line_str_cpp:
                                        continue
                                    if '&' in line_str_cpp:
                                        callback_type = line_str_cpp.split(callback_str)[1].split('&')[0].strip()+'::SharedPtr '
                                        cpp_str = cpp_str.replace(line_str_cpp,
                                                                  line_str_cpp.split(callback_str)[0] + callback_str + callback_type +
                                                                  line_str_cpp.split('&')[1])

                                    elif ';' in line_str_cpp:
                                        # print(line_str_cpp)
                                        msg_datas = line_str_cpp.split(callback_str)[1].split(')')[0].strip()
                                        if msg_datas:
                                            callback_type = 'const ' + msg_datas.split(' ')[0] + '::SharedPtr ' + \
                                                            msg_datas.split(' ')[1]
                                            replace_str = line_str_cpp.split(callback_str)[
                                                                 0] + callback_str + callback_type + ')' + \
                                                             line_str_cpp.split(')')[1].strip()
                                            cpp_str = cpp_str.replace(line_str_cpp,replace_str)
                                            # print(msg_datas)


            for line_str_cpp in cpp_str.split('\n'):
                for pub in pubs:
                    if pub in line_str_cpp and 'rclcpp::Publisher ' in line_str_cpp:
                        if pub in pubs_dic:
                            space = line_str_cpp.split('rclcpp::Publisher ')[0]
                            replace_str = space+ 'rclcpp::Publisher<' + pubs_dic[pub] + '>::SharedPtr ' + line_str_cpp.split('rclcpp::Publisher ')[1]
                            cpp_str = cpp_str.replace(line_str_cpp, replace_str)
                        # print(replace_str)
                    # if pub in line_str_cpp:
                    #     print(line_str_cpp)
                for sub in subs:
                    if sub in line_str_cpp and 'rclcpp::Subscription ' in line_str_cpp:
                        space = line_str_cpp.split('rclcpp::Subscription ')[0]
                        if sub in subs_dic_sub_back.keys():
                            # print(line_str_cpp)
                            if subs_dic_sub_back[sub] not in subs_dic_back_type.keys():
                                # print(line_str_cpp)
                                continue
                            replace_str = space + 'rclcpp::Subscription<' + subs_dic_back_type[subs_dic_sub_back[sub]] + '>::SharedPtr ' + \
                                          line_str_cpp.split('rclcpp::Subscription ')[1]
                            # print(replace_str)
                            cpp_str = cpp_str.replace(line_str_cpp, replace_str)
                        # print(replace_str)

                    if sub+' ' in line_str_cpp and 'node->create_subscription' in line_str_cpp and 'create_subscription<' not in line_str_cpp:
                        if sub in subs_dic_sub_back.keys():
                            head = line_str_cpp.split(sub)[0]
                            if subs_dic_sub_back[sub] not in subs_dic_back_type.keys():
                                # print(line_str_cpp)
                                continue
                            replace_str = head + sub +' = node->create_subscription<' + subs_dic_back_type[
                                subs_dic_sub_back[sub]] +'>'+ \
                                          line_str_cpp.split('node->create_subscription')[1]
                            cpp_str = cpp_str.replace(line_str_cpp, replace_str)


            for line_str_cpp in cpp_str.split('\n'):
                if 'ubt_com' in line_str_cpp:
                    if 'Request' in line_str_cpp or 'Response' in line_str_cpp:
                        replace_str = line_str_cpp.replace('ubt_com::', 'ubt_interface::srv::')
                    else:
                        replace_str = line_str_cpp.replace('ubt_com::', 'ubt_interface::msg::')
                    cpp_str = cpp_str.replace(line_str_cpp, replace_str)

        file = file.replace(input_path, out_path)
        file_dir = file.replace(file.split('/')[-1], '')
        if not os.path.exists(file_dir):
            os.makedirs(file_dir)
        with open(file, 'w', encoding='utf-8') as f:
            f.write(cpp_str)

def read_param_type():
    with open('param_type.txt','r') as f:
        strs = f.readlines()
        for str in strs:
            # print(str)
            type = str.split(' ')[0]
            if type.strip() != '':
                if len(str.split(' '))>=2:
                    var = str.split(' ')[1].split(';')[0]
                else:
                    # print(str)
                    var = str
                    pass

                # print(var+'  :'+type)
                params_var_type[var] = type

if __name__ == '__main__':
    read_param_type()
    get_files_in_folder(input_path)
    ros_to_uauto_launchOrCfg()
    ros_to_uauto_msgOrSrv()
    ros_to_uauto_h()
    ros_to_uauto_cpp()
    get_files_in_folder(input_path)
    ros_to_uauto_other()




